#include "polytope.h"
#include "error_info.h"

namespace Packing_common
{

	Packing_polygon::~Packing_polygon() 
	{
		//delete skeleton_circles;
		CGAL_polygon::clear();
		update_display_list = true;
	}

	void Packing_polygon::redraw(const GLfloat *fill_color) const
	{
		update_display_list = true;
		gl_draw(fill_color);
	}

	double Packing_polygon::squared_distance(const Point& p) const
	{
		double dist = std::numeric_limits<double>::max();
		for (Packing_polygon::Edge_const_iterator eit = edges_begin(); eit != edges_end(); ++eit)
		{
			double edge_dist = CGAL::squared_distance(p, *eit);
			dist = std::min(dist, edge_dist);
		}
		return dist;
	}

	double Packing_polygon::perimeter() const
	{
		double pmt = 0.0;
		for (Edge_const_iterator eit = edges_begin(); eit != edges_end(); ++eit)
			pmt += CGAL::sqrt(eit->squared_length());
		return pmt;
	}
	double Packing_polygon::squared_distance(const Point& p, Vector& gradv, Point& closest_point) const
	{
		double dist = std::numeric_limits<double>::max();
		for (Packing_polygon::Edge_const_iterator eit = edges_begin(); eit != edges_end(); ++eit)
		{
			double d2;
			Vector g;
			// where the projection point lies, get the parameter t. P' = t*S + (1-t)*T
			// for accuracy, use average of x and y coordinate. To avoid division instability, use the algorithm in CGAL::squared_distance_2
			Vector src_p(eit->source(), p), edge_vec(eit->source(), eit->target());
			double dotp = src_p*edge_vec;
			Point closest_pnt;
			if (dotp < 0.0)
			{
				d2 = CGAL::squared_distance(p, eit->source());
				g = 2*Vector(eit->source(), p);
				closest_pnt = eit->source();
			}
			else 
			{
				if (dotp > edge_vec*edge_vec)
				{
					d2 = CGAL::squared_distance(p, eit->target());
					g = 2*Vector(eit->target(), p);
					closest_pnt = eit->target();
				}
				else
				{
					Line_2 l = eit->supporting_line();
					d2 = CGAL::squared_distance(p, l);
					double tmp = 2*(l.a()*p.x()+l.b()*p.y()+l.c())/(l.a()*l.a()+l.b()*l.b());
					g = Vector(tmp*l.a(), tmp*l.b());
					closest_pnt = l.projection(p);
				}
			}
			if (dist > d2)
			{
				dist = d2;
				gradv = g;
				closest_point = closest_pnt;
			}
			//assert( CGAL::squared_distance(p, *eit) == d2);
		}
		return dist;
	}

	void Packing_polygon::gl_draw(const GLfloat *fill_color) const
	{
		//if (update_display_list)
		//{
		//	if (glIsList(display_list_id))
		//		glDeleteLists(display_list_id, 1);

		//	display_list_id = glGenLists(1);
		//	glNewList(display_list_id, GL_COMPILE_AND_EXECUTE);

		glLineWidth(1.0f);

		// draw interior fill draw
		typedef CGAL::Partition_traits_2< CGAL::Exact_predicates_inexact_constructions_kernel >::Polygon_2 Partition_polygon;

		std::list<Partition_polygon> convex_partitions;
		CGAL::approx_convex_partition_2( vertices_begin(), vertices_end(), std::back_inserter(convex_partitions) );

		for (std::list<Partition_polygon>::iterator it = convex_partitions.begin(); it != convex_partitions.end(); ++it)
		{
			glBegin(GL_POLYGON);
			for (Partition_polygon::Vertex_const_iterator vit = it->vertices_begin(); vit != it->vertices_end(); ++vit)
				glVertex2d(vit->x(), vit->y());
			glEnd();
		}

		// draw edges in black
		glColor3f(0.0f, 0.0f, 0.0f);
		glBegin(GL_LINE_LOOP);
		typedef CGAL_polygon::Vertex_const_iterator Con_VIter;
		for (Con_VIter it = vertices_begin(); it != vertices_end(); ++it)
			glVertex2d(it->x(), it->y());
		glEnd();

		//	glEndList();
			
		//	update_display_list = false;
		//}
		//glCallList(display_list_id);

		// draw the skeleton

	}

	void Packing_polygon::gl_draw_circles(const GLfloat *fill_color) const
	{
		if (skeleton != 0)
		{
			glBegin(GL_LINES);
			for (Straight_skeleton_2::Halfedge_const_iterator heit = skeleton->halfedges_begin(); heit != skeleton->halfedges_end(); ++heit)
			{
				if (heit->is_bisector())
				{
					glVertex2d(heit->vertex()->point().x(), heit->vertex()->point().y());
					glVertex2d(heit->opposite()->vertex()->point().x(), heit->opposite()->vertex()->point().y());
				}
			}
			glEnd();
			static const GLfloat color[] = {0.7f, 0.7f, 0.7f};
			std::for_each(skeleton_circles.begin(), skeleton_circles.end(), std::bind2nd(std::mem_fun_ref(Particle_circle::gl_draw), fill_color));
		}
	}
	void Packing_polygon::gl_draw_bbox() const
	{
		Iso_rectangle_2 bdbox = bbox();
		glLineWidth(0.8f);
		glColor3f(1.0f, 0.0f, 0.0f);
		glLineStipple(1, 0xAAAA);
		glEnable(GL_LINE_STIPPLE);
		glBegin(GL_LINE_LOOP);
		glVertex2d(bdbox.xmin(), bdbox.ymin());
		glVertex2d(bdbox.xmax(), bdbox.ymin());
		glVertex2d(bdbox.xmax(), bdbox.ymax());
		glVertex2d(bdbox.xmin(), bdbox.ymax());
		glEnd();
		glDisable(GL_LINE_STIPPLE);
	}
	void Packing_polygon::translate(double dx, double dy)
	{
		CGAL::Aff_transformation_2<K> trans_vec(CGAL::TRANSLATION, CGAL::Vector_2<K>(dx, dy)); 
		CGAL_polygon::operator=(CGAL::transform(trans_vec, *this));
		update_display_list = true;
		//update_skeleton();
	}

	void Packing_polygon::update_skeleton()
	{
		skeleton = CGAL::create_interior_straight_skeleton_2(vertices_begin(), vertices_end()); 
		skeleton_circles.clear();
		std::set<Straight_skeleton_2::Vertex_handle> vertices_added;
		std::set<Straight_skeleton_2::Halfedge_iterator> edges_added;
		// construct the approximating circles
		double skeleton_len = 0.0;
		for (Straight_skeleton_2::Halfedge_iterator heit = skeleton->halfedges_begin(); heit != skeleton->halfedges_end(); ++heit)
		{
			if (!heit->is_bisector())	continue;
			Straight_skeleton_2::Vertex_handle vh0 = heit->vertex(), vh1 = heit->opposite()->vertex();
			skeleton_len += CGAL::sqrt(CGAL::squared_distance(vh0->point(), vh1->point()));
		}
		skeleton_len /= 2; // for counting opposite edges
		//std::cout<<"skeleton length = "<<skeleton_len<<std::endl;
		const unsigned int N = 10; // total sampling points
		for (Straight_skeleton_2::Halfedge_iterator heit = skeleton->halfedges_begin(); heit != skeleton->halfedges_end(); ++heit)
		{
			if (!heit->is_bisector())	continue;
			Straight_skeleton_2::Vertex_handle vh0 = heit->vertex(), vh1 = heit->opposite()->vertex();
			if (edges_added.find(heit) != edges_added.end())
				continue;
			edges_added.insert(heit);
			edges_added.insert(heit->opposite());
			double len = CGAL::sqrt(CGAL::squared_distance(vh0->point(), vh1->point()));
			const unsigned int n = len/skeleton_len * N + 0.5;
			const Straight_skeleton_2::Halfedge_const_handle def_edge = heit->defining_contour_edge();
			K::Line_2 def_line(def_edge->vertex()->point(), def_edge->opposite()->vertex()->point());
			if (vertices_added.find(vh0) == vertices_added.end())
			{
				double d = CGAL::squared_distance(vh0->point(), def_line);
				if (d != 0.0)
					skeleton_circles.push_back(Particle_circle(vh0->point(), d));
				vertices_added.insert(vh0);
			}
			if (vertices_added.find(vh1) == vertices_added.end())
			{
				double d = CGAL::squared_distance(vh1->point(), def_line);
				if (d != 0.0)
					skeleton_circles.push_back(Particle_circle(vh1->point(), d));
				vertices_added.insert(vh1);
			}
			for (unsigned int i = 1; i < n; i++)
			{
				Point p = vh0->point() + Vector(vh0->point(), vh1->point())*i/n;
				double d = CGAL::squared_distance(p, def_line);
				skeleton_circles.push_back(Particle_circle(p, d));
			}
		}
	}
	void Packing_polygon::rotate(double angle)
	{
		if (angle == 0.0)
			return;
		double rad = angle * boost::math::constants::pi<double>()/180.0;
		double s = std::sin(rad), c = std::cos(rad);
		CGAL::Aff_transformation_2<K> rot_mat(CGAL::ROTATION, s, c);
		CGAL_polygon::operator=(CGAL::transform(rot_mat, *this));
		update_display_list = true;
		//update_skeleton();
	}
	void Packing_polygon::relative_scale(double factor)
	{
		assert(factor > 0.0);
		Point c = centroid();
		Parameter::Transformation_2 cent2origin(CGAL::TRANSLATION, Vector(c, CGAL::ORIGIN));
		Parameter::Transformation_2 origin2cent(CGAL::TRANSLATION, Vector(CGAL::ORIGIN, c));
		Parameter::Transformation_2 scale_mat(CGAL::SCALING, factor);
		CGAL_polygon::operator=(CGAL::transform(origin2cent*(scale_mat*cent2origin), *this));
		scale_factor *= factor;
		update_display_list = true;
		//update_skeleton();
	}
	void Packing_polygon::scale(double factor)
	{
		assert(factor > 0.0);
		relative_scale(factor/scale_factor);
	}
	Packing_polygon::Iso_rectangle_2 Packing_polygon::bbox() const
	{
		return CGAL::bounding_box(vertices_begin(), vertices_end());
	}
	void Packing_polygon::self_rotate(double angle)
	{
		if (angle == 0.0)
			return;
		Point_2 cent = CGAL::centroid(vertices_begin(), vertices_end(), CGAL::Dimension_tag<0>());
		CGAL::Aff_transformation_2<K> trans(CGAL::TRANSLATION, CGAL::Vector_2<K>(cent, CGAL::ORIGIN));
		double rad = angle * boost::math::constants::pi<double>()/180.0;
		double s = std::sin(rad), c = std::cos(rad);
		CGAL::Aff_transformation_2<K> rot(CGAL::ROTATION, s, c);
		CGAL::Aff_transformation_2<K> back(CGAL::TRANSLATION, CGAL::Vector_2<K>(CGAL::ORIGIN, cent));
		CGAL::Aff_transformation_2<K> m = back * (rot * trans);
		CGAL_polygon::operator=(CGAL::transform(m, *this));
		update_display_list = true;
		//update_skeleton();
	}

	void Packing_polygon::transform(const Parameter& p)
	{
		Transformation_2 mat = p.to_transformation();
		CGAL_polygon::operator=(CGAL::transform(mat, *this));
		scale_factor *= p.k;
		update_display_list = true;
		//update_skeleton();
	}

	Packing_polygon& Packing_polygon::operator*=(const Packing_polygon::Transformation_2& t)
	{
		CGAL_polygon::operator=(CGAL::transform(t, *this));
		update_display_list = true;
		return *this;
	}

	Packing_polygon transform(const Packing_polygon::Transformation_2& t, const Packing_polygon& p)
	{
		Packing_polygon res;
		for (unsigned int i = 0; i < p.size(); i++)
			res.push_back(t(p.vertex(i)));
		//res.update_skeleton();
		return res;
	}
	Polygon_sampler::Polygon_sampler(const Packing_polygon& polygon_) : polygon(polygon_)
	{
		// add the constraint
		size_t sz = polygon.size();
		CDT::Vertex_handle *vhs = new CDT::Vertex_handle[sz];
		for (size_t i = 0; i < sz; i++)
		{
			vhs[i] = cdt.insert(polygon.vertex(i));
		}
		for (size_t i = 0; i < sz; i++)
		{
			cdt.insert_constraint(vhs[i], vhs[(i+1)%sz]);
		}
		delete[] vhs;
	}

	void Polygon_sampler::inner_sample(size_t ref_pnt_nb, std::vector<Point_2>& pnts)
	{
		pnts.clear();
		//pnts.reserve(ref_pnt_nb);
		double polygon_area = polygon.volume();
		std::set<Point_2> sample_pnts;
		for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin(); fit != cdt.finite_faces_end(); ++fit)
		{
			Point_2 bc = CGAL::barycenter(fit->vertex(0)->point(), 1.0/3, fit->vertex(1)->point(), 1.0/3, fit->vertex(2)->point());
			if (!polygon.has_on_bounded_side(bc))
				continue;
			double tri_area = std::fabs(CGAL::area(fit->vertex(0)->point(), fit->vertex(1)->point(), fit->vertex(2)->point()));
			size_t n = tri_area/polygon_area*ref_pnt_nb;
			if (n == 0)
				n = 1;
			for (int i = 0; i <= n; i++)
			{
				for (int j = n - i; j >= 0; j--)
				{
					double alpha = static_cast<double>(i)/n, beta = static_cast<double>(j)/n;
					Point_2 p = CGAL::barycenter(fit->vertex(0)->point(), alpha, fit->vertex(1)->point(), beta, fit->vertex(2)->point());
					sample_pnts.insert(p);
				}
			}
		}
		pnts.reserve(sample_pnts.size());
		pnts.assign(sample_pnts.begin(), sample_pnts.end());
	}

	void Polygon_sampler::inner_sample(double ref_step, std::vector<Point_2>& pnts) 
	{
		// sample in the interior
		Bounding_box bdbox = CGAL::bounding_box(polygon.vertices_begin(), polygon.vertices_end());
		double y = bdbox.ymin() + ref_step;
		while ( y < bdbox.ymax() )
		{
			double x = bdbox.xmin() + ref_step;
			while ( x < bdbox.xmax() )
			{
				Point_2 p(x, y);
				if (polygon.has_on_bounded_side(p))
					pnts.push_back(p);
				x += ref_step;
			}
			y += ref_step;
		}
		// sample on the boundary
		double prm = polygon.perimeter();
		size_t N = static_cast<size_t>(prm/ref_step) + 1;
		for (Packing_polygon::Edge_const_iterator eit = polygon.edges_begin(); eit != polygon.edges_end(); ++eit)
		{
			double l = CGAL::sqrt(eit->squared_length());
			size_t n = static_cast<size_t>(l/prm*N) + 1;
			Point_2 s = eit->source(), t = eit->target();
			for (size_t i = 0; i < n; i++)
			{
				Point_2 p = CGAL::ORIGIN + (n - i)*Vector_2(CGAL::ORIGIN, s)/n + i*Vector_2(CGAL::ORIGIN, t)/n;
				pnts.push_back(p);
			}
		}

	}
	void Polygon_sampler::edge_sample(size_t ref_pnt_nb, std::vector<Point_2>& pnts)
	{
		pnts.clear();
		pnts.reserve(ref_pnt_nb);
		std::valarray<double> edge_len(polygon.size());
		double perimeter = 0.0;
		for (size_t i = 0; i < polygon.size(); i++)
		{
			edge_len[i] = CGAL::sqrt(polygon.edge(i).squared_length());
			perimeter += edge_len[i];
		}
		for (size_t i = 0; i < polygon.size(); i++)
		{
			unsigned int n = edge_len[i]/perimeter*ref_pnt_nb;
			n = std::max<unsigned int>(n, 1);
			Point_2 src = polygon.edge(i).source(), tgt = polygon.edge(i).target();
			for (unsigned int j = 0; j < n; j++)
			{
				Point_2 p = src + static_cast<double>(j)/n*Vector_2(src, tgt);
				pnts.push_back(p);
			}
		}
	}
}